//All includes
#include "ros/ros.h"
#include <std_msgs/Int32.h>
#include "roboard_drivers/Encoder.h"
#include "roboard_drivers/Motor.h"
#include <unistd.h>
//#include "roboard_drivers/imu.h"
#include <stdio.h>
#include "saint_nr_9/MotCom.h"



using namespace roboard_drivers;

int main(int argc, char **argv){
        ros::init(argc, argv, "MotorKeybd");//Creates a node named "MotorControl"
        ros::NodeHandle n;

	// Define a publisher object instance.
	ros::Publisher pub = n.advertise<saint_nr_9::MotCom>("/control/motor_control", 1); // Don't care about "nh" for now.
	
	ros::Rate loop_rate(10);
	saint_nr_9::MotCom msg;
	char command[10];// = (char*)malloc(10);
	while(ros::ok()){
		printf(">");
		fgets(command, 10, stdin);
		char *c = strtok(command, " ");
		double value = atof(strtok(NULL, " "));

		if(*c == 'w'){
			msg.command = 0;
			msg.dist_or_angle = value;
			msg.speed = 0.5;
		}

                if(*c == 's'){
                        msg.command = 1;
                        msg.dist_or_angle = value;
                        msg.speed = 0.5;
                }

                if(*c == 'a'){
                        msg.command = 2;
                        msg.dist_or_angle = value;
                        msg.speed = 0.5;
                }

                if(*c == 'd'){
                        msg.command = 3;
                        msg.dist_or_angle = value;
                        msg.speed = 0.5;
                }
                
                if(*c == 'q'){
                	return 0;
                }

		pub.publish(msg);
		ros::spinOnce();
		loop_rate.sleep(); 
	}

	return 0;
}


	

